import RPi.GPIO as io
import time

# speed=0-100
# operation=0:Forward,1:Backward,2:TurnLeft,3:TurnRight,4:TurnAround
def move(speed,operation,runtime,wheel,accelerator):
    usePWM=True

    # Enable wheels & press accleerator
    # print("start at "+str(time.time()))
    if usePWM:
        pwm=io.PWM(accelerator,0.5)
        pwm.start(speed)
    else:
        io.output(accelerator,1)

    # Control wheels & sleep
    if operation==0:
        for i in range(4):
            io.output(wheel[0][i],1)
        time.sleep(runtime)
    if operation==1:
        for i in range(4):
            io.output(wheel[1][i],1)
        time.sleep(runtime)
    if operation==2:
        for i in range(4):
            io.output(wheel[1-i%2][i],1)
        time.sleep(1)
    if operation==3:
        for i in range(4):
            io.output(wheel[i%2][i],1)
        time.sleep(1)
    if operation==4:
        for i in range(4):
            io.output(wheel[i%2][i],1)
        time.sleep(2)
    
    # Disable wheels
    # print("stop at "+str(time.time()))
    if usePWM:
        pwm.stop()
    else:
        io.output(accelerator,0)
    for i in range(2):
        for j in range(4):
            io.output(wheel[i][j],0)
    




io.setmode(io.BOARD)

# Init wheels
wheel=[[12,18,32,38],[16,22,36,40]]
accelerator=37

for i in range(2):
    for j in range(4):
        # print("init"+str(wheel[i][j]))
        io.setup(wheel[i][j],io.OUT,initial=io.LOW)
io.setup(accelerator,io.OUT,initial=io.LOW)

# Run
move(10,0,1.5,wheel,accelerator)

# Exit
io.cleanup()